#include "Control.h"
#include "pid.h"

//pid初始化第12行，循迹计算第26行，速度计算第40行

/*BianLiang ShengMing Begin*/
float Velocity,Encoder_Least,Encoder,Movement;
float Encoder_Integral,Target_Velocity;
/*BianLiang ShengMing End*/

/**************************************************************************
函数功能：pid初始化
入口参数：pid结构体指针
返回  值：无
**************************************************************************/
void PID_Init(){
	PID_Straight.Kp=-100;//-110
	PID_Straight.Kd=19;//19
	PID_Trace.Kp=110/45;
	PID_Trace.Kd=19;

	PID_HighSpeed_Trace.Kp=2.09;//110/45
	PID_HighSpeed_Trace.Kd=19.3;//19
	PID_BasicSpeed_Trace.Kp=1.38;//0.98
	PID_BasicSpeed_Trace.Kd=15.3;//18.3
	PID_LowSpeed_Trace.Kp=2.38;
	PID_LowSpeed_Trace.Kd=11.8;
}

/**************************************************************************
函数功能：循迹pid的计算（包括直行pid）
入口参数：pid结构体指针，循迹目标的偏差
返回  值：pid计算结果，返回给电机
**************************************************************************/
float PID_Trace_Calc(tPID *PIDX,float Yaw_Bias,float G_Bias){

	PIDX->output=PIDX->Kp*Yaw_Bias+PIDX->Kd*G_Bias; //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数

	if(PIDX->output > 4900 )   PIDX->output = 4900;
	if(PIDX->output < -4900 )  PIDX->output = -4900;

	return PIDX->output;
}

/*
float PID_Speed_Calc(tPID *PIDX) {

}
*/